/**********************************************************
 * ROS通讯和MPC控制的入口
 * @author:HuYunhao
 * @email:hnu_huyunhao1995@163.com
 * @date:2021/05/24
 * */


#include <iostream>
#include <vector>
#include <ros/ros.h>
#include "ros_mpc/mpc.h"            //自定义的ros消息头
#include "polyfit.h"                //曲线拟合
#include "matplotlibcpp.h"          //绘图库
#include "mpc_controller.h"
#include "path_plan.h"              //规划模块

using Eigen::VectorXd;
namespace plt = matplotlibcpp;

// ROS消息接收和发送的变量，接收来自PreScan的数据
ros_mpc::mpc mpc_msg_rx;        // mpc msg receive
ros_mpc::mpc mpc_msg_tx;        // mpc msg transmit

// ROS消息订阅的回调函数，接收来自PreScan的信息
void chatterCallback(const ros_mpc::mpcConstPtr &msg) {
    ROS_INFO("I receive [x, y, yaw, v]=[%f, %f, %f, %f]", msg->x, msg->y, msg->yaw, msg->v);
    mpc_msg_rx = *msg;
}

// ROS的主调函数，用来初始化ROS节点，发布和订阅ROS消息
void runRos(int argc, char **argv) {
    // 初始化ROS节点，并给节点取名
    ros::init(argc, argv, "n_mpc_controller");
    // 创建一个句柄， 用以操作ROS节点
    ros::NodeHandle n;
    // 发布ROS消息,其中control是ros topic的名字，1000是队列的size
    ros::Publisher publisher = n.advertise<ros_mpc::mpc>("control", 1000);
    // 订阅ROS消息
    ros::Subscriber subscriber = n.subscribe("preScan", 1000, chatterCallback);
    // 本节点发布ROS topic的周期
    ros::Rate loop_rate(100);       //100HZ
    // MPC控制器的相关操作
    // 1创建MPC控制器
    MPC mpc;
    // 2接收来自规划的信息，并且使用polyFit拟合规划路径点
    pathPlan_NS::DesiredPath desiredPath;
    VectorXd coeffs = desiredPath.coeffs_;
    // 3.定义车辆的初始位置，计算车辆和路径的初始横向偏差和航向偏差
    pathPlan_NS::State car(9.34, 21.99,0,0);
    car.calError(desiredPath.coeffs_);
    // 进入ros循环
    while (ros::ok()){
        ros::spinOnce();
        vector<double> result = mpc.Solve(car.state_, desiredPath.coeffs_);
        double delta = result[6];   //车轮转角[deg]
        mpc_msg_tx.delta = myNumpy::rad2deg(delta) * 16;                //车轮转角*传动比
        mpc_msg_tx.error_front_axle = result[4];
        ROS_INFO("I send delta[%f]", mpc_msg_tx.delta);
        // 更新states的值
//        car.state_ << result[0], result[1], result[2], result[3], result[4], result[5];
        car.state_ << mpc_msg_rx.x, mpc_msg_rx.y, mpc_msg_rx.yaw, mpc_msg_rx.v, result[4], result[5];
        publisher.publish(mpc_msg_tx);
        loop_rate.sleep();
    }

}

void test01() {
    using std::cout;
    using std::endl;
    using std::vector;
    // 1创建MPC控制器
    MPC mpc;
    // 2接收来自规划的信息，并且使用polyFit拟合规划路径点
    pathPlan_NS::DesiredPath desiredPath;
    VectorXd coeffs = desiredPath.coeffs_;

    // 3.定义车辆的初始位置，计算车辆和路径的横向偏差和航向偏差
    pathPlan_NS::State car(-27, 42,0,10);
    car.calError(desiredPath.coeffs_);

    // 3.1将车辆初始化变量放入states向量中
    VectorXd states(6);
    states << car.x_,car.y_,car.phi_,car.v_,car.cte_,car.ephi_;

    // 3.2建立一些vector，储存计算值
    vector<double> x_vals = {states[0]};
    vector<double> y_vals = {states[1]};
    vector<double> phi_vals = {states[2]};
    vector<double> v_vals = {states[3]};
    vector<double> cte_vals = {states[4]};
    vector<double> ephi_vals = {states[5]};
    vector<double> delta_vals = {};
    vector<double> a_vals = {};

    // 进入控制循环，计算控制量[delta, a]
    for (size_t i = 0; i < 60; i++) {
        cout << "第" << i << "次循环" << endl;
        auto vars = mpc.Solve(states, coeffs);
        // 储存控制的计算值
        x_vals.push_back(vars[0]);
        y_vals.push_back(vars[1]);
        phi_vals.push_back(vars[2]);
        v_vals.push_back(vars[3]);
        cte_vals.push_back(vars[4]);
        ephi_vals.push_back(vars[5]);

        delta_vals.push_back(vars[6]);
        a_vals.push_back(vars[7]);

        // 更新车辆的当前状态值
        states << vars[0], vars[1], vars[2], vars[3], vars[4], vars[5];
        // 打印当前控制的计算结果
        cout << "x = " << vars[0] << endl;
        cout << "y = " << vars[1] << endl;
        cout << "phi = " << vars[2] << endl;
        cout << "v = " << vars[3] << endl;
        cout << "cte = " << vars[4] << endl;
        cout << "ephi = " << vars[5] << endl;
        cout << "delta = " << vars[6] << endl;
        cout << "a = " << vars[7] << endl;
        cout << endl;
    }
    // 绘图
    plt::subplot(2, 2, 1);
    plt::title("cross_track_error");
    plt::plot(cte_vals);

    plt::subplot(2, 2, 2);
    plt::title("Delta[rad]");
    plt::plot(delta_vals);

    plt::subplot(2, 2, 3);
    plt::title("Velocity");
    plt::plot(v_vals);

    plt::subplot(2, 2, 4);
    plt::title("x-y");
    plt::plot(x_vals, y_vals);
    plt::show();
}

int main(int argc, char **argv) {
    runRos(argc, argv);
//    test01();
    return 0;
}
